#include <arm/interrupt.h>
#include <arm/timer.h>
#include <kernel.h>
#include <config.h>
#include <exports.h>
#include <arm/reg.h>
#include <arm/psr.h>

void irq_handler();

volatile unsigned long ticks;

unsigned int irq_stack[IRQ_STACK_SIZE];

unsigned int icmr, iclr, oscr, osmr0, osmr1, osmr2, osmr3, oier;
int install_handler(unsigned int * vector_addr, void (* handler)(), unsigned int * uboot_ptr)
{
        if(LDR_OPCODE(*vector_addr) != LDR_INSTR)       {
        //not LDR instruction
                return -1;
        }

        int offset = LDR_OFFSET(*vector_addr);

        if(!LDR_POS_OFFSET(*vector_addr))       {
        //negative offset
                offset = (0 - offset);
        }

        //retrieving address of uboot swi handler
    //add 0x8 to offset + address of current instruction 
    unsigned int jt_addr = (unsigned int)(offset + (unsigned int)vector_addr + 0x8);
        unsigned int * jump_table_index = (unsigned int *) jt_addr;

        unsigned int * uboot_handler = (unsigned int *)*(jump_table_index);

    /*storing uboot_handler's instructions */
    uboot_ptr[0] = (unsigned int)uboot_handler;
    uboot_ptr[1] = *(uboot_handler);


        /*modifying instruction to transfer control 
                to our handler*/
        *(uboot_handler) = ((LDR_INSTR & 0xFF7FFFFF) | 4);
        uboot_handler ++;
    uboot_ptr[2] = *(uboot_handler);
        *(uboot_handler) = (unsigned int)handler;


        return 0;
}



void activate_timer()
{
    icmr = reg_read(INT_ICMR_ADDR);
    reg_write(INT_ICMR_ADDR, 0);	
    

    /* Activate OS Timer 0 */
    reg_set(INT_ICMR_ADDR, (1 << INT_OSTMR_0));

    /* Deliver timer interrupts as IRQ */
    iclr = reg_read(INT_ICLR_ADDR);
    reg_clear(INT_ICLR_ADDR, (1 << INT_OSTMR_0));
}

int setup_irq(unsigned int * uboot_i)
{
    if(install_handler((unsigned int *)IRQ_VECTOR, irq_wrapper, uboot_i) < 0)  {
        return -1;
    }
    
    int count = (OSTMR_FREQ / OS_TICKS_PER_SEC); //OSTMR_FREQ * 10 ms
    
    /*save values of Timer registers in global variables*/
    osmr0 = reg_read(OSTMR_OSMR_ADDR(0));
    osmr1 = reg_read(OSTMR_OSMR_ADDR(1));
    osmr2 = reg_read(OSTMR_OSMR_ADDR(2));
    osmr3 = reg_read(OSTMR_OSMR_ADDR(3));

    reg_write(OSTMR_OSMR_ADDR(0), count); //Set Timer Match register to count
    reg_write(OSTMR_OSMR_ADDR(1), 0);
    reg_write(OSTMR_OSMR_ADDR(2), 0);
    reg_write(OSTMR_OSMR_ADDR(3), 0);

    oscr = reg_read(OSTMR_OSCR_ADDR);
    reg_write(OSTMR_OSCR_ADDR, 0);

    activate_timer();

    /* Set bit E0 in OIER */

    oier = reg_read(OSTMR_OIER_ADDR);
    reg_write(OSTMR_OIER_ADDR, OSTMR_OIER_E0);

    return 0;
}

void irq_handler()
{
    unsigned int ossr = reg_read(OSTMR_OSSR_ADDR);
    /* check if bit M0 of OSSR is 1 */
    if(ossr & OSTMR_OSSR_M0)   {
        ticks ++; //incremented every 10ms
        int count = (OSTMR_FREQ / OS_TICKS_PER_SEC); //OSTMR_FREQ * 10 ms
        reg_write(OSTMR_OSMR_ADDR(0), count);
        reg_write(OSTMR_OSCR_ADDR, 0); //clear OSCR
        reg_write(OSTMR_OSSR_ADDR, OSTMR_OSSR_M0);
    }
}
